﻿#include "pathelement.h"
#include "global.h"

static const double pi = 3.1415926;

PathElement::PathElement() : pathElementType(ERR),
    beginPoint(Point()), endPoint(Point()), midPoint(Point()),
    beginJoint(Joint()), endJoint(Joint()), midJoint(Joint())
{

}



PathElement::PathElement(Joint begin, Joint end, PathElementType type) : pathElementType(type),
    beginJoint(begin), endJoint(end), midPoint(Point()), midJoint(Joint())
{

    this->beginPoint = Joint2Point(beginJoint);
    this->endPoint = Joint2Point(endJoint);

}


PathElement::PathElement(Joint begin, Joint mid, Joint end, PathElementType type) : pathElementType(type),
    beginJoint(begin), midJoint(mid), endJoint(end)
{

    this->beginPoint = Joint2Point(beginJoint);
    this->midPoint = Joint2Point(mid);
    this->endPoint = Joint2Point(endJoint);

}


#if 0
PathElement::PathElement(Point begin, Point end, PathElementType type): pathElementType(type),
    beginPoint(begin), endPoint(end), midPoint(Point())
{

    this->beginJoint = Point2Joint(beginPoint);
    this->endJoint = Point2Joint(endPoint);

}
#endif


PathElement::~PathElement()
{

#ifndef NDEBUG
    qDebug() << "~PathElement()";
#endif

    /// 智能指针指向null就会析构原来的内存
    //ifKinematicsHandle = nullptr

}

/** 根据关节角度获取工具尖点坐标 */
Point PathElement::Joint2Point(const Joint& joint) const
{

    Eigen::Vector3d p = {0, 0, 0};

    p = ifKinematicsHandle->getPositionInWorld(joint);

    return Point(p[0], p[1], p[2]);

}



/** 根据工具尖点坐标获取关节角度 */
Joint PathElement::Point2Joint(const Point& point) const
{
    return Joint();

}




std::vector<Joint> PathElement::getJointArray() const
{
    std::vector<Joint> jointArray;
    std::vector<Eigen::Matrix4d> poseArray;

    if (pathElementType == MoveLine) {
        poseArray = linearPoseAndPositionInterpolation();
    } else if (pathElementType == MoveArc) {
        poseArray = arcPoseAndPositionInterpolation();
    }

    /// 遍历位姿数组,转换为joint数组
    for (auto poseMatrix: poseArray) {
        jointArray.push_back(pose_2_Joint(poseMatrix));
    }

    return jointArray;
}



Joint PathElement::pose_2_Joint(Eigen::Matrix4d poseMatrix) const
{

    Joint joint;

    joint = ifKinematicsHandle->iKine(poseMatrix);

#if 0
    /// 打印所有插补的关节角
    qDebug() <<"===========================";

    qDebug() << poseMatrix(0, 3) << poseMatrix(1, 3) << poseMatrix(2, 3);
    joint.print();
    Eigen::Matrix4d mat = ifKinematicsHandle->fKine(joint);
    qDebug() << mat(0, 3) << mat(1, 3) << mat(2, 3);


#endif

    /// 更新上一个位置
    lastJoint = joint;

    return joint;

}




/** 直线位置和姿态插补算法
 * @return 插补得到的位姿矩阵4*4数组
 */
std::vector<Eigen::Matrix4d> PathElement::linearPoseAndPositionInterpolation() const
{

    /// 位姿矩阵数组
    std::vector<Eigen::Matrix4d> postMatrixArray;

    /// 获取起点和终点
    Eigen::Vector3d begin(this->beginPoint.x, this->beginPoint.y, this->beginPoint.z);
    Eigen::Vector3d end(this->endPoint.x, this->endPoint.y, this->endPoint.z);

    /// 获取起点终点的位姿矩阵
    Eigen::Matrix4d beginPoseMatrix = getPoseMatrix(beginJoint);
    Eigen::Matrix4d endPoseMatrix = getPoseMatrix(endJoint);

    /// 获取起点和终点旋转矩阵的四元数
    Eigen::Quaterniond beginQuat = poseMatrix_2_quaterniond(beginPoseMatrix);
    Eigen::Quaterniond endQuat = poseMatrix_2_quaterniond(endPoseMatrix);

    /// 计算两点距离
    Eigen::Vector3d vec = end - begin;
    double length = sqrt(vec.dot(vec));

    /// 按1mm精度插补
    int cnt = length / 1;

#ifndef NDEBUG
    qDebug() << QString(QStringLiteral("直线长度:%1")).arg(length);
    qDebug() << QString(QStringLiteral("插补次数:%1")).arg(cnt);
#endif

    for (int i = 0; i <= cnt; ++i) {

        /// 插补系数t
        double t = i / static_cast<double>(cnt);

        /// 线性姿态插补获取中间点的姿态
        Eigen::Quaterniond tmp = poseInterpolation(beginQuat, endQuat, t);
        /// 位置插补获取中间点空间位置
        Eigen::Vector3d position = linearPositionInterpolation(begin, end, t);

        /// 将中间点姿态转换为4*4的位姿矩阵
        Eigen::Matrix4d poseMatrix = quaterniond_2_poseMatrix(tmp);
        /// 更新位姿矩阵的空间位置坐标
        poseMatrix(0, 3) = position[0];
        poseMatrix(1, 3) = position[1];
        poseMatrix(2, 3) = position[2];

        /// 插入数组
        postMatrixArray.push_back(poseMatrix);

#ifndef NDEBUG
        /// 打印所有插补点
        //qDebug() << QString(QStringLiteral("坐标:(%1, %2, %3)")).arg(position[0]).arg(position[1]).arg(position[2]);
#endif

    }

    return postMatrixArray;

}



/** 圆弧位置和姿态插补算法
 * @return 插补得到的位姿矩阵4*4数组
 */
std::vector<Eigen::Matrix4d> PathElement::arcPoseAndPositionInterpolation() const
{

    /// 位姿矩阵数组
    std::vector<Eigen::Matrix4d> postMatrixArray;

    /// 获取起点和终点
    Eigen::Vector3d begin(this->beginPoint.x, this->beginPoint.y, this->beginPoint.z);
    Eigen::Vector3d mid(this->midPoint.x, this->midPoint.y, this->midPoint.z);
    Eigen::Vector3d end(this->endPoint.x, this->endPoint.y, this->endPoint.z);

    /// 获取起点终点的位姿矩阵
    Eigen::Matrix4d beginPoseMatrix = getPoseMatrix(beginJoint);
    Eigen::Matrix4d endPoseMatrix = getPoseMatrix(endJoint);

    /// 获取起点和终点旋转矩阵的四元数
    Eigen::Quaterniond beginQuat = poseMatrix_2_quaterniond(beginPoseMatrix);
    Eigen::Quaterniond endQuat = poseMatrix_2_quaterniond(endPoseMatrix);

    /// 计算圆弧弧度和弧长
    double arcLen = calculateArcLength(begin, mid, end);

    /// 按1mm精度插补
    int cnt = arcLen / 1;

#ifndef NDEBUG
    qDebug() << QString(QStringLiteral("圆弧长度:%1")).arg(arcLen);
    qDebug() << QString(QStringLiteral("插补次数:%1")).arg(cnt);
#endif

    for (int i = 0; i <= cnt; ++i) {

        /// 插补系数t
        double t = i / static_cast<double>(cnt);

        /// 线性姿态插补获取中间点的姿态
        Eigen::Quaterniond tmp = poseInterpolation(beginQuat, endQuat, t);
        /// 位置插补获取中间点空间位置
        Eigen::Vector3d position = arcPositionInterpolation(begin, mid, end, t);

        /// 将中间点姿态转换为4*4的位姿矩阵
        Eigen::Matrix4d poseMatrix = quaterniond_2_poseMatrix(tmp);
        /// 更新位姿矩阵的空间位置坐标
        poseMatrix(0, 3) = position[0];
        poseMatrix(1, 3) = position[1];
        poseMatrix(2, 3) = position[2];

        /// 插入数组
        postMatrixArray.push_back(poseMatrix);

#ifndef NDEBUG
        /// 打印所有插补点
        //qDebug() << QString(QStringLiteral("坐标:(%1, %2, %3)")).arg(position[0]).arg(position[1]).arg(position[2]);
#endif

    }

    return postMatrixArray;

}


/** 计算向量长度 */
static double len(Eigen::Vector3d vec)
{
    return sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
}


double PathElement::calculateArcLength(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3) const
{

    double x1 = p1[0],
           x2 = p2[0],
           x3 = p3[0];
    double y1 = p1[1],
           y2 = p2[1],
           y3 = p3[1];
    double z1 = p1[2],
           z2 = p2[2],
           z3 = p3[2];

    double a1 = (y1*z2 - y2*z1 - y1*z3 + y3*z1 + y2*z3 - y3*z2),
           b1 = -(x1*z2 - x2*z1 - x1*z3 + x3*z1 + x2*z3 - x3*z2),
           c1 = (x1*y2 - x2*y1 - x1*y3 + x3*y1 + x2*y3 - x3*y2),
           d1 = -(x1*y2*z3 - x1*y3*z2 - x2*y1*z3 + x2*y3*z1 + x3*y1*z2 - x3*y2*z1);

    double a2 = 2 * (x2 - x1),
           b2 = 2 * (y2 - y1),
           c2 = 2 * (z2 - z1),
           d2 = x1*x1 + y1*y1 + z1*z1 - x2*x2 - y2*y2 - z2*z2;

    double a3 = 2 * (x3 - x1),
           b3 = 2 * (y3 - y1),
           c3 = 2 * (z3 - z1),
           d3 = x1*x1 + y1*y1 + z1*z1 - x3*x3 - y3*y3 - z3*z3;

    double cx = -(b1*c2*d3 - b1*c3*d2 - b2*c1*d3 + b2*c3*d1 + b3*c1*d2 - b3*c2*d1)
            /(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
    double cy =  (a1*c2*d3 - a1*c3*d2 - a2*c1*d3 + a2*c3*d1 + a3*c1*d2 - a3*c2*d1)
            /(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);
    double cz = -(a1*b2*d3 - a1*b3*d2 - a2*b1*d3 + a2*b3*d1 + a3*b1*d2 - a3*b2*d1)
            /(a1*b2*c3 - a1*b3*c2 - a2*b1*c3 + a2*b3*c1 + a3*b1*c2 - a3*b2*c1);

    auto center = Eigen::Vector3d(cx, cy, cz);

    double pNorm = len((p1-p2).cross(p2-p3));
    double r = (len(p1-p2) * len(p2-p3) * len(p3-p1)) / (2.0 * pNorm);

    double theta = getAngle(p1, p2, p3, center);

    return r * abs(theta);

}


/**  计算三点圆弧各夹角
* @p1 p2 p3 三个点，圆弧方向按照经过点p1p2p3的顺序
* @return 返回三个值， 分别表示两两之间的夹角
*/
double PathElement::getAngle(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, Eigen::Vector3d center) const
{
    Eigen::Vector3d p1c = p1 - center;
    Eigen::Vector3d p2c = p2 - center;
    Eigen::Vector3d p3c = p3 - center;

    double cosTheta = p1c.dot(p3c) / (len(p1c) * len(p3c));
    if (cosTheta >= 1) {
        cosTheta = 1;
    } else if (cosTheta <= -1) {
        cosTheta = -1;
    }
    double theta = acos(cosTheta);

    double cosTheta1 = p1c.dot(p2c) / (len(p1c) * len(p2c));
    if (cosTheta1 >= 1) {
        cosTheta1 = 1;
    } else if (cosTheta1 <= -1) {
        cosTheta1 = -1;
    }
    double theta1 = acos(cosTheta1);

    if (((p2 - p1).cross(p3 - p2).normalized()).dot((p1c.cross(p3 - p1)).normalized()) < 0) {
        theta = theta - 2 * pi;
        //再判断第一个点的夹角是否大于180
        if ((p1c.cross(p2c)).dot(p1c.cross(p3c)) > 0) {
            theta1 -= 2 * pi;
        }
    }
    double theta2 = theta - theta1;
    Eigen::Vector3d v(theta, theta1, theta2);

    return theta;

}



/*
 * 圆弧轨迹位置插补
 * @p1 圆弧起点
 * @p2 圆弧中点
 * @p3 圆弧终点
 * @t 中间点系数
 * @return 返回中间的位置坐标
 */
Eigen::Vector3d PathElement::arcPositionInterpolation(Eigen::Vector3d p1, Eigen::Vector3d p2, Eigen::Vector3d p3, double t) const
{

    double pNorm = len((p1 - p2).cross(p2 - p3));
    double R = (len(p1 - p2)*len(p2 - p3)*len(p3 - p1)) / (2.0 * pNorm);

    double alfa = ((len(p2 - p3)*len(p2 - p3)) * ((p1 - p2).dot(p1 - p3))) / (2 * pNorm*pNorm);
    double beta = ((len(p1 - p3)*len(p1 - p3)) * ((p2 - p1).dot(p2 - p3))) / (2 * pNorm*pNorm);
    double gama = ((len(p1 - p2)*len(p1 - p2)) * ((p3 - p1).dot(p3 - p2))) / (2 * pNorm*pNorm);

    Eigen::Vector3d pc = alfa * p1 + beta * p2 + gama * p3;
    Eigen::Vector3d p1c = p1 - pc;
    Eigen::Vector3d p2c = p2 - pc;
    Eigen::Vector3d p3c = p3 - pc;

    double cosTheta = p1c.dot(p3c) / (len(p1c)*len(p3c));
    if (cosTheta >= 1) {
        cosTheta = 1;
    } else if (cosTheta <= -1) {
        cosTheta = -1;
    }
    double theta = acos(cosTheta);

    double cosTheta1 = p1c.dot(p2c) / (len(p1c)*len(p2c));
    if (cosTheta1 >= 1) {
        cosTheta1 = 1;
    } else if (cosTheta1 <= -1) {
        cosTheta1 = -1;
    }
    double theta1 = acos(cosTheta1);
    if (((p2 - p1).cross(p3 - p2).normalized()).dot((p1c.cross(p3 - p1)).normalized()) < 0) {
        theta = theta - 2*pi;
        //再判断第一个点的夹角是否大于180
        if ((p1c.cross(p2c)).dot(p1c.cross(p3c)) > 0) {
            theta1 -= 2*pi;
        }
    }

    Eigen::Vector3d temp;
    if (theta == pi) {
        temp = p1c.cross(p2c).cross(p1c);
    } else {
        temp = p1c.cross(p3c).cross(p1c);
    }

    Eigen::Vector3d pt = pc + p1c*cos(t * theta) + temp.normalized() * R * sin(t * theta);

    return pt;

}




/** 位姿矩阵转换为四元数
 * @pose 4*4的姿态矩阵
 * @return 返回位姿矩阵中3*3旋转矩阵(或者叫姿态矩阵)的四元数
*/
Eigen::Quaterniond PathElement::poseMatrix_2_quaterniond(const Eigen::Matrix4d& pose) const
{

    Eigen::Matrix3d rotateMatrix = Eigen::Matrix3d::Ones();
    rotateMatrix << pose(0, 0), pose(0, 1), pose(0, 2),
                    pose(1, 0), pose(1, 1), pose(1, 2),
                    pose(2, 0), pose(2, 1), pose(2, 2);

    Eigen::Quaterniond ret;
    ret = rotateMatrix;
    ret.normalized();
    return ret;
}




/** 四元数转换为位姿矩阵
 * @quat 姿态矩阵的四元数
 * @return 返回的是4*4的位姿矩阵, 其中的旋转矩阵(或者叫姿态矩阵)是由四元数转换来的,而位置坐标初始化为(0,0,0)
*/
Eigen::Matrix4d PathElement::quaterniond_2_poseMatrix(const Eigen::Quaterniond& quat) const
{

    Eigen::Matrix4d poseMatrix = Eigen::Matrix4d::Ones();

    /// 四元数转换为3*3的旋转矩阵
    Eigen::Matrix3d rotate = quat.toRotationMatrix();

    poseMatrix << rotate(0, 0), rotate(0, 1), rotate(0, 2),     0,
                  rotate(1, 0), rotate(1, 1), rotate(1, 2),     0,
                  rotate(2, 0), rotate(2, 1), rotate(2, 2),     0,
                             0,            0,            0,     1;

    return poseMatrix;

}




